package com.hitqz.robot.dispatch.netty.strategy.publicPathStrategy.util;

import cn.hutool.core.collection.CollUtil;
import cn.hutool.core.collection.CollectionUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.nacos.client.naming.utils.CollectionUtils;
import com.google.common.cache.Cache;
import com.google.common.collect.HashBasedTable;
import com.google.common.collect.Maps;
import com.google.common.collect.Table;
import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
import com.hitqz.robot.api.business.dto.RobotCommonPathDto;
import com.hitqz.robot.api.business.dto.RobotLockPos;
import com.hitqz.robot.api.business.entity.RobotEntity;
import com.hitqz.robot.api.business.entity.RobotGroupEntity;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import com.hitqz.robot.biz.service.RobotLockPathService;
import com.hitqz.robot.biz.service.RobotLockPosService;
import com.hitqz.robot.dispatch.netty.util.JtsUtil;
import lombok.extern.slf4j.Slf4j;
import org.apache.commons.compress.utils.Lists;
import org.locationtech.jts.geom.LineString;
import org.locationtech.jts.geom.Point;
import org.locationtech.jts.geom.Polygon;

import java.util.*;
import java.util.concurrent.Executors;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.ThreadPoolExecutor;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicReference;
import java.util.stream.Collectors;

/**
 * @author xupkun
 * @date 2024/9/8
 */
@Slf4j
public class PublicPathStrategyUtil {


    private static final ThreadPoolExecutor threadPoolExecutor= new ThreadPoolExecutor(
                    200,
                            200,
                            60,
                    TimeUnit.SECONDS,
                    new LinkedBlockingQueue<Runnable>(200),
                    Executors.defaultThreadFactory(),
                            new ThreadPoolExecutor.DiscardOldestPolicy());

    /**
     * 机器人行驶的目标点/目标区域--被占用，其他机器人先暂停等待
     */
    public static Map<String, String> ROBOT_LOCK_TARGET_POS = Maps.newConcurrentMap();

    /**
     * 机器人当前行驶暂停标志
     */
    public static Map<String, Boolean> ROBOT_STOP_FLAG = Maps.newConcurrentMap();

    /**
     * 机器人-公共路径/区域 - 暂停标志
     */
    public static Map<String, Map<String, Boolean>> ROBOT_COMMON_PATH_STOP_FLAG = Maps.newConcurrentMap();

    /**
     * 机器人到达点位
     */
    public static Map<String, RobotMapPosDto> ROBOT_ARRIVE_POS = Maps.newConcurrentMap();

    /**
     * 机器人下个点位
     */
    public static Map<String, RobotMapPosDto> ROBOT_NEXT_POS = Maps.newConcurrentMap();

    /**
     * 机器人上一个点位
     */
    public static Map<String, RobotMapPosDto> ROBOT_LAST_POS = Maps.newConcurrentMap();


    /**
     * 当前行驶队列--记录整体队列，用于判断机器人是否到达某个点位，到达点位后，会删除
     */
    public static Map<String, LinkedList<RobotMapPosDto>> ROBOT_PATH_LIST = Maps.newConcurrentMap();


    private static final Map<String, RobotApplyPathDto> ROBOT_TARGET_POS_MAP = Maps.newHashMap();

    /**
     * key  机器人_公共路径  进入状态：0-未进入，1-即将进入，2-已进入，3-即将离开，4-已离开
     */
    public static Map<String, Integer> ROBOT_COMMON_PATH_STATUS = Maps.newConcurrentMap();

    public static Table<Integer, String, RobotCommonPathDto> COMMON_PATH_MAP = HashBasedTable.create();


    /**
     * 机器人进入公共路径状态  未进入
     */
    public static Integer ROBOT_COMMON_PATH_NAVER_IN=0;
    /**
     * 机器人进入公共路径状态  已经进入
     */
    public static Integer ROBOT_COMMON_PATH_IN=1;
    /**
     * 机器人进入公共路径状态  离开
     */
    public static Integer ROBOT_COMMON_PATH_LEAVE=2;
    private static final String lockA = "path_list_lock";
    private static final String lockB = "path_list_action_lock";

    /**
     * 行驶
     * @param
     * @param robotApplyPathDto
     */
    public static void travel(RobotGroupEntity group, RobotApplyPathDto robotApplyPathDto) {
        LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
        String robotSn = robotApplyPathDto.getClientId();
        ROBOT_STOP_FLAG.put(robotSn, false);
        ROBOT_COMMON_PATH_STOP_FLAG.remove(robotSn);
        ROBOT_ARRIVE_POS.put(robotSn, pathList.get(0));
        ROBOT_NEXT_POS.put(robotSn, pathList.get(1));
        ROBOT_LAST_POS.put(robotSn, pathList.get(0));
        ROBOT_TARGET_POS_MAP.put(robotSn, robotApplyPathDto);
        RobotLockPathService robotLockPathService = SpringUtil.getBean(RobotLockPathService.class);
        RobotLockPosService robotLockPosService = SpringUtil.getBean(RobotLockPosService.class);
        robotLockPosService.deleteByRobotSn(robotSn);
        robotLockPathService.deleteByRobotSn(robotSn);
        //目标点位是否被锁定
        threadPoolExecutor.execute(new CheckDistPosLockThread(group, robotApplyPathDto));
    }

    /**
     * 校验目标点位是否被占用，占用则等待
     */
    static class  CheckDistPosLockThread extends Thread {
        /**
         * 机器人编码
         */
        private final RobotApplyPathDto robotApplyPathDto;
        private final RobotGroupEntity robotGroup;

        CheckDistPosLockThread(RobotGroupEntity robotGroup, RobotApplyPathDto robotApplyPathDto) {
            this.robotApplyPathDto = robotApplyPathDto;
            this.robotGroup = robotGroup;
        }

        @Override
        public void run() {
            try {
                //开始行驶之前，判断目标点位是否被占用，如果被占用，则先不发开始，等待目标点可用，才下发开始指令
                boolean cancelTask = false;
                while (true) {
                    //判断目标点是否被占用
                    Boolean targetPosLock = false;
                    Iterator iterator = ROBOT_LOCK_TARGET_POS.entrySet().iterator();
                    while (iterator.hasNext()) {
                        Map.Entry entry = (Map.Entry) iterator.next();
                        String value = (String) entry.getValue();
                        String keyName = (String) entry.getKey();
                        if (!keyName.equalsIgnoreCase(robotApplyPathDto.getClientId())
                                && value.equalsIgnoreCase(robotApplyPathDto.getCode())) {
                            log.info("{} 准备前往目标点被锁定了 {} -- {}", robotApplyPathDto.getClientId(), keyName, value);
                            targetPosLock = true;
                            break;
                        }
                    }
                    // 判断是否被其他机器人位置占领
                    if (!targetPosLock) {
                        // 计算的是目标点位/路径上是否存在其他AGV
                        Cache<Integer, List<RobotEntity>> posInfoDtoCache = SpringUtil.getBean("robotDispatch");
                        // 机器人位置的矩形

                        List<RobotEntity> robotEntityList = posInfoDtoCache.getIfPresent(robotGroup.getId());
                        if (robotEntityList == null) {
                            return;
                        }
                        if (robotEntityList.size() > 1) {
                            robotEntityList = robotEntityList.stream().filter(t -> !(t.getRobotSn().equals(robotApplyPathDto.getClientId()))).toList();
                            AtomicReference<String> lockRobot = new AtomicReference<>("");
                            Point point = JtsUtil.getPoint(robotApplyPathDto.getCodeX(), robotApplyPathDto.getCodeY());
                            robotEntityList.forEach(t -> {
                                Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
                                Polygon polygon = robotPosPolygon.getIfPresent(t.getRobotSn());
                                assert polygon != null;
                                if (polygon.contains(point)) {
                                    lockRobot.set(t.getRobotSn());
                                }
                                // 查看路上是否被其他车堵住
                                List<RobotMapPosDto> resetPath  = ROBOT_PATH_LIST.get(robotApplyPathDto.getClientId());
                                if (CollectionUtils.isEmpty(resetPath)) {
                                    resetPath = Lists.newArrayList(); //robotApplyPathDto.getPathList();
                                }

                                for (int i = 0; i < resetPath.size() - 1; i++) {
                                    RobotMapPosDto start = resetPath.get(i);
                                    RobotMapPosDto end = resetPath.get(i + 1);
                                    LineString lineSegment = JtsUtil.createLineString(start.getPosX(), start.getPosY(),
                                            end.getPosX(), end.getPosY());
                                    if (lineSegment.intersects(polygon)) {
                                        log.info("{} 有车在路上挡住需要停止： {}", t.getRobotSn(), robotApplyPathDto.getClientId());
                                        lockRobot.set(t.getRobotSn());
                                        break;
                                    }
                                }
                            });
                            if (StrUtil.isNotBlank(lockRobot.get())) {
                                targetPosLock = true;
                            }
                        }
                    }
                    if (targetPosLock) {
                        //log.info("目标被锁定等待可以前往: {}", robotApplyPathDto.getClientId());
                        Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
                        RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotApplyPathDto.getClientId());
                        if (robotClientSummaryDto != null) {
                            if (robotClientSummaryDto.getStatus().getNavStatus().equals(2)) {
                                log.info("目标被锁定下发等待: {}", robotApplyPathDto.getClientId());
                                //PublicPathStrategyUtil.changeNavStatus(robotApplyPathDto.getClientId(), 1);
                            }
                        }
                        Thread.sleep(1000);
                        //tryCount++;
                    }
                    //锁定目标点/区域
                    if (!cancelTask && !targetPosLock) {
                        ROBOT_LOCK_TARGET_POS.put(robotApplyPathDto.getClientId(), robotApplyPathDto.getCode());
                        //存储行驶路径
                        PublicPathStrategyUtil.getOrSetPathList(robotApplyPathDto.getClientId(),
                                robotApplyPathDto.getPathList(), null, null, true);
                        //匹配公共路径
                        threadPoolExecutor.execute(()->{
                            PublicPathStrategyUtil.findCommonPathThread(robotGroup.getId(), robotApplyPathDto.getClientId());
                        });

                        //开始执行
                        Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
                        RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotApplyPathDto.getClientId());
                        if (robotClientSummaryDto.getStatus().getNavStatus().equals(3) || robotClientSummaryDto.getStatus().getNavStatus().equals(6)) {
                            //log.info("导航被暂停了,下发继续");
                            //PublicPathStrategyUtil.changeNavStatus(robotApplyPathDto.getClientId(), 2);
                        }
                        ROBOT_LOCK_TARGET_POS.put(robotApplyPathDto.getClientId(), robotApplyPathDto.getCode());
                        Thread.sleep(1000);
                        posInfoDtoCache = SpringUtil.getBean("robotCache");
                        robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotApplyPathDto.getClientId());
                        if (StrUtil.isEmpty(robotClientSummaryDto.getPosition().getLaserPos().getTargetPosCode()) &&
                                robotClientSummaryDto.getStatus().getNavStatus().equals(4)
                            || ROBOT_PATH_LIST.get(robotApplyPathDto.getClientId()).isEmpty()
                        ) {
                            log.info("导航到达清除了任务" + robotApplyPathDto.getClientId());
                            break;
                        }

                    }
                }
            } catch (Exception e) {
                log.error("CheckDistPosLockThread-校验AGV行驶目标点占用线程异常：" + e.getMessage());
            }
        }
    }




    public static void getOrSetPathList(String robotSn, LinkedList<RobotMapPosDto> robotPathList, Double posx, Double posy, Boolean setFlag) {
        synchronized (lockA) {
            if (setFlag) {
                //log.info("{}提交路径：{}",
                //        robotSn, robotPathList.stream().map(RobotMapPosDto::getCode)
                //                .collect(Collectors.joining("|")));

                ROBOT_PATH_LIST.put(robotSn, robotPathList);
            } else {
                List<RobotMapPosDto> currentPathList = ROBOT_PATH_LIST.get(robotSn);
                if (currentPathList == null || currentPathList.isEmpty()) {
                    return;
                }
                // 到达点位
                Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
                if (posInfoDtoCache == null) {
                    return;
                }
                try {
                    RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
                    if (robotClientSummaryDto == null) {
                        return;
                    }

                    String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
                    String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
                    if (StrUtil.isEmpty(nearStation) && StrUtil.isEmpty(lastArrivePos)) {
                        return;
                    }

                    int pathListSize = currentPathList.size();
                    int arrivePosIndex = -1;
                    for (int i = 0; i < pathListSize; i++) {
                        if (lastArrivePos.equalsIgnoreCase(currentPathList.get(i).getCode())) {
                            arrivePosIndex = i;
                            break;
                        }
                    }

                    if (arrivePosIndex == -1) {
                        return;
                    }

                    RobotMapPosDto arrivePos = currentPathList.get(arrivePosIndex);
                    ROBOT_ARRIVE_POS.put(robotSn, arrivePos);

                    if (pathListSize > (arrivePosIndex + 1)) {
                        ROBOT_NEXT_POS.put(robotSn, currentPathList.get(arrivePosIndex + 1));
                    } else {
                        // 处理长度为 1 的情况
                        ROBOT_NEXT_POS.remove(robotSn);
                    }

                    RobotMapPosDto lastPos = currentPathList.get(Math.max(0, arrivePosIndex - 1));
                    ROBOT_LAST_POS.put(robotSn, lastPos);
                    // 增加同步控制
                    synchronized (lockB) {
                        currentPathList.remove(arrivePosIndex);
                        ROBOT_PATH_LIST.put(robotSn, new LinkedList<>(currentPathList));
                    }
                    //log.info("****{}-机器人经过点位-{}--{}", robotSn, arrivePos);
                    log.info("------------{} 剩余的路径------------------ {}",robotSn ,currentPathList.stream().map(RobotMapPosDto::getCode)
                            .collect(Collectors.joining("|")));
                } catch (Exception e) {
                    log.error("Error processing robot path: {}", e.getMessage(), e);
                }
            }
        }
    }

    private static List<RobotMapPosDto> findCommonElementsOptimized(List<RobotMapPosDto> list1, List<RobotMapPosDto> list2) {
        Set<RobotMapPosDto> set = new HashSet<>(list2);
        return list1.stream()
                .filter(set::contains)
                .collect(Collectors.toList());
    }

    public static void findCommonPathThread(Integer groupId,String robotSn) {
        RobotApplyPathDto firstRobot = ROBOT_TARGET_POS_MAP.get(robotSn);
        List<RobotMapPosDto> firstPath = firstRobot.getPathList();
        for (String key : ROBOT_TARGET_POS_MAP.keySet()) {
            if (key.equalsIgnoreCase(robotSn)) {
                continue;
            }
            RobotApplyPathDto nextRobotPath = ROBOT_TARGET_POS_MAP.get(key);
            List<RobotMapPosDto> keyPath = nextRobotPath.getPathList();
            List<RobotMapPosDto> commonPath = findCommonElementsOptimized(firstPath,keyPath);
            List<RobotMapPosDto> commonPath2 = findCommonElementsOptimized(keyPath, firstPath);
            //如果真实路径无公共路径，则找出路径点上转弯点的临近点，组成路径，再算公共路径
            if (CollectionUtil.isNotEmpty(commonPath)) {
                // 查询方向
                RobotMapPosDto startPos = commonPath.get(0);
                RobotMapPosDto endPos = commonPath.get(commonPath.size() - 1);
                RobotCommonPathDto commonPathDto = new RobotCommonPathDto();
                commonPathDto.setRobotSnA(robotSn);
                commonPathDto.setAvoidNum(0);
                commonPathDto.setRobotSnB(key);
                commonPathDto.setStartPos(startPos);
                commonPathDto.setEndPos(endPos);
                commonPathDto.setSameDirection(false);
                commonPathDto.setCommonPathName(UUID.randomUUID().toString());
                if (startPos.getCode().equalsIgnoreCase(endPos.getCode()) ||
                        (startPos.getCode().equalsIgnoreCase(commonPath2.get(0).getCode())
                                && endPos.getCode().equalsIgnoreCase(commonPath2.get(commonPath2.size() - 1).getCode()))) {
                    //同向
                    commonPathDto.setSameDirection(true);
                }
                StringBuilder commonPosCodes = new StringBuilder();
                for (int i = 0; i < commonPath.size(); i++) {
                    String commonPos = commonPath.get(i).getCode();
                    commonPosCodes.append(commonPos).append("-");
                }
                commonPathDto.setCommonPathPos(commonPosCodes.toString());
                COMMON_PATH_MAP.put(groupId, commonPathDto.getCommonPathName(), commonPathDto);
                PublicPathStrategyUtil.ROBOT_COMMON_PATH_STATUS.put(robotSn + "_" + commonPathDto.getCommonPathName(), 0);
                PublicPathStrategyUtil.ROBOT_COMMON_PATH_STATUS.put(key + "_" + commonPathDto.getCommonPathName(), 0);
            }
        }
    }


    public static void dispatchMonitor(Integer groupId) {

    }


    public static void finishedPath(Integer groupId, String robotSn) {
        RobotClientSummaryDto summaryDto = PublicPathStrategyUtil.getClientSummary(robotSn);
        if (summaryDto != null) {
            String nearCode = summaryDto.getPosition().getLaserPos().getNearPosCode();
            RobotMapPosDto nextCode = ROBOT_NEXT_POS.get(robotSn);
            if (nextCode != null && StrUtil.isNotEmpty(nearCode)) {
                if (nearCode.equals(nextCode.getCode())) {
                    try {
                        //删除锁
                        RobotLockPathService robotLockPathService = SpringUtil.getBean(RobotLockPathService.class);
                        RobotLockPosService robotLockPosService = SpringUtil.getBean(RobotLockPosService.class);
                        robotLockPathService.deleteByRobotSn(robotSn);
                        robotLockPosService.deleteByRobotSn(robotSn);
                        Iterator iterator = COMMON_PATH_MAP.row(groupId).entrySet().iterator();
                        while (iterator.hasNext()) {
                            Map.Entry entry = (Map.Entry) iterator.next();
                            RobotCommonPathDto commonPathDto = (RobotCommonPathDto) entry.getValue();
                            if (commonPathDto.getRobotSnA().equals(robotSn) || commonPathDto.getRobotSnB().equals(robotSn)) {
                                robotLockPathService.deleteByCommonPath(commonPathDto.getCommonPathName());
                                robotLockPosService.deleteByRobotSn(commonPathDto.getRobotSnA());
                                robotLockPosService.deleteByRobotSn(commonPathDto.getRobotSnB());
                                ROBOT_COMMON_PATH_STATUS.remove(commonPathDto.getRobotSnA() + "_" + commonPathDto.getCommonPathName());
                                ROBOT_COMMON_PATH_STATUS.remove(commonPathDto.getRobotSnB() + "_" + commonPathDto.getCommonPathName());
                                iterator.remove();
                            }
                        }
                        ROBOT_PATH_LIST.remove(robotSn);
                        ROBOT_ARRIVE_POS.remove(robotSn);
                        ROBOT_NEXT_POS.remove(robotSn);
                        ROBOT_LAST_POS.remove(robotSn);
                    }catch (Exception e){
                        log.error("{}-巡线队列执行完成通知,处理公共锁异常：{}",robotSn,e);
                    }

                }
            }
        }

    }

    /**
     * 判断AGV在公共路径的位置，返回申请公共路径的优先级--计算当前AGV位置离公共路径的距离
     *
     * @param commonPathDto    公共路径对象
     * @param robotSn          AGV编码
     * @param robotPathList    路径点集合
     * @param robotA_arrivePos 当前到达点位
     * @return
     */
    private static Double getCommonLockLevel(RobotCommonPathDto commonPathDto, String robotSn, List<RobotMapPosDto> robotPathList, RobotMapPosDto robotA_arrivePos, RobotMapPosDto robotA_nextPos) {
        String key = robotSn + "_" + commonPathDto.getCommonPathName();
        RobotClientSummaryDto clientSummary = PublicPathStrategyUtil.getClientSummary(robotSn);
        if (Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_NAVER_IN)) {
            if (commonPathDto.getCommonPathPos().contains(robotA_arrivePos + "-")) {
                //已进入，直接返回 0.0D
                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_IN);
                return 0.0D;
            }
            RobotMapPosDto startPos = commonPathDto.getStartPos();
            RobotMapPosDto endPos = commonPathDto.getEndPos();
            Double dist = 0.0;
            Boolean leave = true; //是否离开公共路径
            if (robotPathList != null && !robotPathList.isEmpty() && robotPathList.size() > 1) {
                //计算AGV当前位置到公共路径起点的距离
                dist = Math.abs(Math.sqrt((clientSummary.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) * (clientSummary.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) + (clientSummary.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY()) * (clientSummary.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY())));
                for (int i = 1; i < robotPathList.size() - 1; i++) {
                    RobotMapPosDto pos = robotPathList.get(i);
                    if (pos.getCode().equalsIgnoreCase(startPos.getCode())
                            || pos.getCode().equalsIgnoreCase(endPos.getCode())) {
                        leave = false;
                        break;
                    }
                    dist += Math.abs(Math.sqrt((pos.getPosX() - robotPathList.get(i + 1).getPosX()) * (pos.getPosX() - robotPathList.get(i + 1).getPosX()) + (pos.getPosY() - robotPathList.get(i + 1).getPosY()) * (pos.getPosY() - robotPathList.get(i + 1).getPosY())));
                }
            }
            if (leave) {
                //AGV已离开公共路径，直接返回 10.0D
                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_LEAVE);
                return 10.0D;
            }
            if (Double.compare(dist, 5) <= 0) {
                //如果当前AGV离公共路径起点的距离小于避障距离，则认为已进入，直接返回 0.0D
                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_IN);
                dist = 0.0D;
            }
            return dist;
        } else if (Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_IN)) {
            Boolean startPos = false,endPos=false,leaveFlag=false;
            //判断AGV是否离开公共路径
            for (RobotMapPosDto pathPos : CollUtil.emptyIfNull(robotPathList)) {
                if (pathPos.getCode().equalsIgnoreCase(commonPathDto.getStartPos().getCode())) {
                    startPos = true;
                }
                if(pathPos.getCode().equalsIgnoreCase(commonPathDto.getEndPos().getCode())){
                    endPos=true;
                }
            }
//            if (startPos && !endPos) {
//                if (!robotA_arrivePos.getWheelPos() && !robotA_nextPos.getWheelPos() && robotA_arrivePos.getCode().equalsIgnoreCase(commonPathDto.getStartPos().getCode())) {
////                    log.info("{}***1-1包含公共路径起点，且公共路径起点就是AGV当前到达点位，且当前点与下个点都不是转弯点，则表示AGV离开公共路径",robotSn);
//                    leaveFlag=true;
//                } else if (!robotA_arrivePos.getWheelPos() && robotA_nextPos.getWheelPos() && robotA_arrivePos.getCode().equalsIgnoreCase(commonPathDto.getStartPos().getCode())) {
////                    log.info("{}***1-2包含公共路径起点，且公共路径起点就是AGV当前到达点位，当前点不是转弯点，下个点是转弯点，若当前点与下个转弯点距离大于机器人避障距离，则表示AGV离开公共路径",robotSn);
//                    Double dist_to_next= Math.abs(Math.sqrt((robotA_arrivePos.getPosx() - robotA_nextPos.getPosx()) * (robotA_arrivePos.getPosx() - robotA_nextPos.getPosx()) + (robotA_arrivePos.getPosy() - robotA_nextPos.getPosy()) * (robotA_arrivePos.getPosy() - robotA_nextPos.getPosy())));
//                    Robot robot = robotService.findByRobotSn(robotSn);
//                    if (Double.compare(dist_to_next, robot.getObstacleAvoidDis()) >= 0) {
//                        leaveFlag=true;
//                    }
//                }
//            }else if (!startPos && endPos) {
//                if (!robotA_arrivePos.getWheelPos() && !robotA_nextPos.getWheelPos() && robotA_arrivePos.getCode().equalsIgnoreCase(commonPathDto.getEndPos().getCode())) {
////                    log.info("{}***2-1包含公共路径终点，且公共路径终点就是AGV当前到达点位，且当前点与下个点都不是转弯点，则表示AGV离开公共路径",robotSn);
//                    leaveFlag=true;
//                } else if (!robotA_arrivePos.getWheelPos() && robotA_nextPos.getWheelPos() && robotA_arrivePos.getCode().equalsIgnoreCase(commonPathDto.getEndPos().getCode())) {
////                    log.info("{}***2-2包含公共路径终点，且公共路径终点就是AGV当前到达点位，当前点不是转弯点，下个点是转弯点，若当前点与下个转弯点距离大于机器人避障距离，则表示AGV离开公共路径",robotSn);
//                    Double dist = Math.abs(Math.sqrt((robotA_arrivePos.getPosx() - robotA_nextPos.getPosx()) * (robotA_arrivePos.getPosx() - robotA_nextPos.getPosx()) + (robotA_arrivePos.getPosy() - robotA_nextPos.getPosy()) * (robotA_arrivePos.getPosy() - robotA_nextPos.getPosy())));
//                    Robot robot = robotService.findByRobotSn(robotSn);
//                    if (Double.compare(dist, robot.getObstacleAvoidDis()) >= 0) {
//                        leaveFlag=true;
//                    }
//                }
//            }else 
            if (!startPos && !endPos) {
                //不包含公共路径点，则为离开公共路径
                leaveFlag=true;
            }
            if(leaveFlag){
                //AGV已离开公共路径，直接返回 10.0D
                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_LEAVE);
                return 10.0D;
            }
            return 0.0D;
        } else if (Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_LEAVE)) {
            return 10.0D;
        }
        return 99D;
    }

    public static void changeNavStatus(String clientId, int status) {
        //log.info("下发导航状态为:" + status);
        SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(clientId, status));
        //SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(clientId, status));
    }


    /**
     * 同向行驶，AGV优先级相同，判断哪台AGV更靠近公共路径终点（AGV在前，先申请）
     *
     * @param commonPathDto
     * @param robotSn
     * @return
     */
    private static Double getToEndCodeDist(RobotCommonPathDto commonPathDto, String robotSn, List<RobotMapPosDto> robotPathList) {
        Double distEnd = 0.0;
        String key = robotSn + "_" + commonPathDto.getCommonPathName();
        if ( ROBOT_COMMON_PATH_STATUS.get(key) != null
                && Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_IN)) {
            RobotMapPosDto endPos = commonPathDto.getEndPos();
            if (!robotPathList.isEmpty() && robotPathList.size() > 0) {
                if (robotPathList.get(0).getCode().equalsIgnoreCase(endPos.getCode())) {
                    return distEnd;
                }
                RobotClientSummaryDto summaryDto = PublicPathStrategyUtil.getClientSummary(robotSn);
                distEnd = Math.abs(Math.sqrt((summaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) * (summaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) + (summaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY()) * (summaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY())));
                for (int i = 1; i < robotPathList.size() - 1; i++) {
                    RobotMapPosDto pos = robotPathList.get(i);
                    if (pos.getCode().equalsIgnoreCase(endPos.getCode())) {
                        break;
                    } else {
                        distEnd += Math.abs(Math.sqrt((pos.getPosX() - robotPathList.get(i + 1).getPosX()) * (pos.getPosX() - robotPathList.get(i + 1).getPosX()) + (pos.getPosY() - robotPathList.get(i + 1).getPosY()) * (pos.getPosY() - robotPathList.get(i + 1).getPosY())));
                    }
                }
            }
        }
        return distEnd;
    }


    /**
     * 同向行驶，AGV优先级相同，判断哪台AGV更靠近公共路径终点（AGV在前，先申请）
     *
     * @param commonPathDto
     * @param robotSn
     * @return
     */
    private static Double getToStartCodeDist(RobotCommonPathDto commonPathDto, String robotSn, List<RobotMapPosDto> robotPathList) {
        Double distEnd = 0.0;
        RobotClientSummaryDto summaryDto = PublicPathStrategyUtil.getClientSummary(robotSn);
        if (summaryDto != null) {
            if (!robotPathList.isEmpty()) {
                if (robotPathList.get(0).getCode().equalsIgnoreCase(commonPathDto.getStartPos().getCode())
                        || robotPathList.get(0).getCode().equalsIgnoreCase(commonPathDto.getEndPos().getCode())) {
                    return distEnd;
                }
                distEnd = Math.abs(Math.sqrt((summaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) * (summaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) + (summaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY()) * (summaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY())));
                for (int i = 1; i < robotPathList.size() - 1; i++) {
                    RobotMapPosDto pos = robotPathList.get(i);
                    if (pos.getCode().equalsIgnoreCase(commonPathDto.getStartPos().getCode()) || pos.getCode().equalsIgnoreCase(commonPathDto.getEndPos().getCode())) {
                        break;
                    }
                    distEnd += Math.abs(Math.sqrt((pos.getPosX() - robotPathList.get(i + 1).getPosX()) * (pos.getPosX() - robotPathList.get(i + 1).getPosX()) + (pos.getPosY() - robotPathList.get(i + 1).getPosY()) * (pos.getPosY() - robotPathList.get(i + 1).getPosY())));
                }
            }
        }
        return distEnd;
    }



    /**
     * 申请公共路径锁资源 —— 同向
     *
     * @param robotPathList  路径点集合
     * @param commonPathDto  公共路径对象
     * @param robotSn        AGV编码
     * @param lockLevel      路权（距离公共路径起点的距离）
     * @param deleteFlag     是否可以删除点位锁表的数据
     * @param frontRobotSn   前一台AGV
     * @param frontRobotLock 前一台AGV的路权
     */
    private static void applySameDirectionLock(Integer groupId, List<RobotMapPosDto> robotPathList, RobotCommonPathDto commonPathDto, String robotSn, Double lockLevel, Boolean deleteFlag, String frontRobotSn, Double frontRobotLock) {
        try {
            if (robotPathList == null || robotPathList.size() <= 1) {
                return;
            }
            Map<String, Boolean> commonPthStop = new HashMap<>();
            if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
            }
            if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) {
                commonPthStop.put(commonPathDto.getCommonPathName(), false);
            }
            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
            //AGV离开公共路径，则删除相关点位锁，返回
            if (ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()) != null
                    && Objects.equals(ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()), ROBOT_COMMON_PATH_LEAVE)) {
                RobotLockPosService robotLockPosService = SpringUtil.getBean(RobotLockPosService.class);
                robotLockPosService.deleteByRobotSnAndCommpathName(robotSn, commonPathDto.getCommonPathName());
                return;
            }
            if (Double.compare(lockLevel, Double.valueOf(7.0)) > 0) {
                return;
            }
            Boolean frontRobotStop = false;
            //如果同向前一台机器人已经进入公共路径，且停止前进，则当前机器人也停止；
            if (!robotSn.equalsIgnoreCase(frontRobotSn) && Double.compare(frontRobotLock, 0.0) == 0 && ROBOT_COMMON_PATH_STOP_FLAG.containsKey(frontRobotSn)) {
                frontRobotStop = ROBOT_COMMON_PATH_STOP_FLAG.get(frontRobotSn).get(commonPathDto.getCommonPathName());
            }
            if (frontRobotStop != null && frontRobotStop) {
                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
                }
                if ((COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null )&&
                        (!commonPthStop.containsKey(commonPathDto.getCommonPathName()) || !commonPthStop.get(commonPathDto.getCommonPathName()))) {
                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
                }
                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
                return;
            }
            RobotClientSummaryDto robotClientSummaryDto = PublicPathStrategyUtil.getClientSummary(robotSn);
            Double distLock = Math.abs(Math.sqrt((robotClientSummaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) * (robotClientSummaryDto.getPosition().getLaserPos().getX() - robotPathList.get(1).getPosX()) + (robotClientSummaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY()) * (robotClientSummaryDto.getPosition().getLaserPos().getY() - robotPathList.get(1).getPosY())));
            //同向行驶，锁前进点位，尝试插入，成功，则可前进，失败，则停止前进
            List<RobotLockPos> lockPosList = new ArrayList<>();
            for (int i = 1; i < robotPathList.size(); i++) {
                RobotMapPosDto pos = robotPathList.get(i);
                RobotLockPos lockPos = new RobotLockPos();
                lockPos.setRobotSn(robotSn);
                lockPos.setCommonPath(commonPathDto.getCommonPathName());
                lockPos.setLockPos(pos.getCode());
                lockPosList.add(lockPos);
                if (i < robotPathList.size() - 1) {
                    distLock += Math.abs(Math.sqrt((pos.getPosX() - robotPathList.get(i + 1).getPosX()) * (pos.getPosX() - robotPathList.get(i + 1).getPosX()) + (pos.getPosY() - robotPathList.get(i + 1).getPosY()) * (pos.getPosY() - robotPathList.get(i + 1).getPosY())));
                    if (Double.compare(distLock, Double.valueOf(7.0)) >= 0) {
                        break;
                    }
                }
            }
            RobotLockPosService robotLockPosService = SpringUtil.getBean(RobotLockPosService.class);
            if (robotLockPosService.batchInsert(robotSn, commonPathDto.getCommonPathName(), lockPosList, deleteFlag) <= 0) {
                //申请路径点位锁失败，则暂停标记为TRUE,并返回
                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
                }
                if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null) {
                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
                }
                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
            }
        } catch (Exception e) {
            log.error("申请锁资源程序异常：{}", e);
        }
    }

    /**
     * 申请公共路径锁资源 —— 反向
     *
     * @param robotPathList 路径点集合
     * @param commonPathDto 公共路径对象
     * @param robotSn       AGV编码
     * @param lockLevel     路权（距离公共路径起点的距离）
     * @param deleteFlag    是否可以删除点位锁表的数据
     * @param stopFlag      暂停标记（如果公共路径的另一台AGV已经优先行驶，则当前AGV直接暂停）
     */
    private static void applyReverseLock(Integer groupId, List<RobotMapPosDto> robotPathList, RobotCommonPathDto commonPathDto, String robotSn, Double lockLevel, Boolean deleteFlag, Boolean stopFlag) {
        try {
            if (robotPathList == null || robotPathList.size() <= 1) {
                return;
            }
            Map<String, Boolean> commonPthStop = new HashMap<>();
            if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
            }
            if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) {
                commonPthStop.put(commonPathDto.getCommonPathName(), false);
            }
            //AGV离开公共路径，直接返回
            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
            if (ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()) != null
                    && Objects.equals(ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()), ROBOT_COMMON_PATH_LEAVE)) {
                RobotLockPathService robotLockPathService = SpringUtil.getBean(RobotLockPathService.class);
                robotLockPathService.deleteByRobotSnAndCommpathName(robotSn, commonPathDto.getCommonPathName());
                return;
            }
            if (Double.compare(lockLevel, Double.valueOf(7)) > 0) {
                return;
            }
            //如果公共路径另一台AGV优先行驶，则当前AGV暂停标记stopFlag为true，并且未进入公共路径，则直接暂停标记，并返回
            if (stopFlag && Double.compare(lockLevel, 0) > 0) {
                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
                }
                if (COMMON_PATH_MAP.get(groupId,commonPathDto.getCommonPathName()) != null) {
                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
                }
                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
                return;
            }
            //未进入公共路径，申请加锁，如果插入失败，则标记暂停
            RobotLockPathService robotLockPathService = SpringUtil.getBean(RobotLockPathService.class);
            if (robotLockPathService.insert(robotSn, commonPathDto, deleteFlag) <= 0) {
                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
                }
                if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) {
                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
                }
                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
            }
            if (robotLockPathService.insert(robotSn, commonPathDto, deleteFlag) <= 0) {
                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
                }
                if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null) {
                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
                }
                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
            }
        } catch (Exception e) {
            log.error("申请锁资源程序异常：{}", e);
        }
    }


    public static RobotClientSummaryDto getClientSummary(String clientId) {
        Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
        return posInfoDtoCache.getIfPresent(clientId);
    }
}
